using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace XnaDevRu.BulletX
{
	/// <summary>
	/// EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to
	/// calculate the penetration depth between two convex shapes.
	/// </summary>
	public class GjkEpaPenetrationDepthSolver : IConvexPenetrationDepthSolver
	{
		public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, IDebugDraw debugDraw)
		{
			float radialmargin = 0;

			GjkEpaSolver.Results results;
			if (GjkEpaSolver.Collide(convexA, transformA,
									 convexB, transformB,
									 radialmargin, out results))
			{
				//	debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
				//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
				ptrA = results.Witnesses[0];
				ptrB = results.Witnesses[1];
				return true;
			}
			ptrA = new Vector3();
			ptrB = new Vector3();

			return false;
		}
	}
}
